#!/usr/bin/env python
# -*- coding: UTF-8 -*-
'''
@Project ：bignet_sumo_dev
@File ：main-dev-tongluoshan.py
@IDE ：PyCharm
@Author ：xinyingjie
@Date ：2025/3/24 9:40
'''

from __future__ import absolute_import
from __future__ import print_function

import asyncio
import math
import queue
import json
import threading
import random
import time
from multiprocessing import Process, Queue

import numpy as np
import traci.constants as tc
import sumolib

from kafka import KafkaProducer, KafkaConsumer
from pyproj import Proj

from config.config import GLOBAL_POLYGON_CENTER, MAX_SPEED, KAFKA_HOST, \
    RADAR_DATA_LIST_JJ as RADAR_DATA_LIST, KAFKA_E1_TOPIC, KAFKA_E1_TOPIC_SIMU, SHOW_SIMU, USE_LIBSUMO, USE_GUI, KAFKA_E1_LIST, LIDAR_RANGE, \
    PINJIE_DIS, HM_LIDAR_RANGE
from utils import sumo_utils
from loguru import logger
from utils.tools import calculate_distance, find_vehicles_by_duration_optimized, \
    calculate_vehicle_acceleration_deceleration, get_timestamp_milliseconds, time_str, \
    get_timestamp_milliseconds_jingtai, get_next_target_car_info_jingtai, is_in_front_jingtai
import concurrent.futures

from websocket.websocketserver1 import WebSocketServer

if USE_LIBSUMO:
    import libsumo as traci
else:
    import traci

logger.add("/sumo/debug.log", format="{time} {level} {message}", level="DEBUG")


def read_trafficE1_channel(target_channel, q_r, kafka_queue, queue_data, match_queue_data, static_queue_data):
    # 实时接收全域E1帧数据
    kafka_consumer_E1 = KafkaConsumer(
        target_channel,
        bootstrap_servers=KAFKA_HOST,
        auto_offset_reset='latest',
        enable_auto_commit=False
    )
    kafka_producer = KafkaProducer(bootstrap_servers=KAFKA_HOST, key_serializer=str.encode,
                                   value_serializer=lambda x: json.dumps(x).encode('utf-8'),
                                   compression_type='gzip')
    # ws_server = WebSocketServer()
    # ws_server.start()
    # loop = asyncio.new_event_loop()  # 创建新的事件循环
    # asyncio.set_event_loop(loop)
    logger.info(f"全域E1 {target_channel} 监听已就绪")
    count_add = 0

    start_time = int(time.time() * 1000)
    kafka_queue.put([target_channel, int(time.time())])
    # 缓存感知11帧数据
    cache_data = Queue(maxsize=2000)
    simu_json_list_before = []
    match_json_dict = None
    statisticInfo = {"totalNum": 0, "totalNumTypeInfo": [], 'meanSpeed': 0}
    while True:
        time.sleep(0.1)
        for message in kafka_consumer_E1:
            if message:
                try:
                    sn_set = set()
                    one_list = []
                    message_value = message.value.decode('utf-8')
                    data = json.loads(message_value)
                    q_r.put(data.copy())
                    simu_json_list = []
                    while not queue_data.empty():
                        simu_json_list = queue_data.get()

                    while not match_queue_data.empty():
                        match_json_dict = match_queue_data.get()
                    while not static_queue_data.empty():
                        statisticInfo = static_queue_data.get()
                        # print(statisticInfo)
                    if 'targetList' not in data.keys():
                        continue
                    e1FrameParticipant_list = data['targetList']
                    ts = int(time.time() * 1000)
                    if abs(ts - start_time) > 1000 * 120:
                        start_time = ts
                        kafka_queue.put([target_channel, int(time.time())])
                    # temp_list = []
                    # for one in e1FrameParticipant_list:
                    #     sourceId = one['station']
                    #     if match_json_dict and sourceId in match_json_dict.keys() and one['id'] in match_json_dict[
                    #         sourceId].keys():
                    #         # logger.info(f"感知数据可以修改")
                    #         one['id'] = match_json_dict[sourceId][one['id']]['id'].split("-")[0]
                    #     temp_list.append(one)
                    # data_c = data.copy()
                    # data_c['targetList'] = temp_list
                    # q_r.put(data_c)
                    for one in e1FrameParticipant_list:
                        sourceId = one['station']
                        one['sourceId'] = sourceId
                        # if sourceId % 2 != 0:
                        sn_set.add(sourceId)
                        # if sourceId != 34:
                        car_id = one['id']
                        dis = (calculate_distance(RADAR_DATA_LIST[sourceId]['lon'],
                                                  RADAR_DATA_LIST[sourceId]['lat'],
                                                  one['lon'], one['lat']))
                        if RADAR_DATA_LIST[sourceId]['type'] == 'jg':
                            target_range = LIDAR_RANGE
                        else:
                            target_range = HM_LIDAR_RANGE
                        if dis <= target_range:

                            # temp = {"id": str(one['id']), "type": 1, 'source': 4,
                            #         "laneNum": 0,
                            #         "longitude": one['lon'], "latitude": one['lat'], "speed": int(30 * 3.6),
                            #         "courseAngle": round(one['angle'], 1), 'plate': "默A00000", "flag": 0,
                            #         'orgCode': one['orgCode']
                            #         }
                            if match_json_dict and sourceId in match_json_dict.keys() and str(one['id']) in \
                                    match_json_dict[
                                        sourceId].keys():
                                # logger.info(f"感知数据可以修改 {temp['id']} -》 {match_json_dict[sourceId][str(one['id'])]['id']}")
                                one['id'] = int(str(match_json_dict[sourceId][str(one['id'])]['id']).strip("-")[0])
                            else:
                                one['id'] = int(str(one['id']).split("-")[0])
                                # temp['flag'] = 999
                            one_list.append(one)
                            # if match_json_dict and sourceId in match_json_dict.keys():
                            #     logger.info(f"感知数据可以修改 {str(one['id']) in match_json_dict[sourceId].keys()}")
                            #     # temp['id'] = match_json_dict[sourceId][str(one['id'])]['id']
                            #     # temp['flag'] = 2
                            # one_list.append(temp)
                    cache_data.put(one_list)
                    one_list_his = []
                    while cache_data.qsize() > 25:
                        one_list_his = cache_data.get()
                    if len(simu_json_list) > 0:
                        simu_json_list_before = simu_json_list
                    if SHOW_SIMU:
                        data['targetList'] = one_list_his + simu_json_list_before if len(
                            simu_json_list_before) > 0 else one_list_his
                    else:
                        data['targetList'] = one_list_his
                    # del data['targetList']
                    data['statisticInfo'] = statisticInfo
                    print(data)

                    kafka_producer.send(KAFKA_E1_TOPIC_SIMU, value=data,
                                        key=str(int(time.time())))
                    # future = asyncio.run_coroutine_threadsafe(
                    #     ws_server.broadcast(str(data).replace("'", '"')),
                    #     ws_server.loop)
                    # future.result()  # 等待协程完成

                except Exception as e:
                    logger.error(str(e))
                    continue


class SumoMain:
    def __init__(self, sumo_net_file, sumo_cfg_file, gui, process_id):
        """
        仿真初始化
        :param sumo_net_file:  仿真路网文件路径
        :param sumo_cfg_file:  仿真配置文件路径
        :param gui: 是否开启gui
        :param process_id: 进程id
        """

        self.sumo_net_file = sumo_net_file
        self.sumo_cfg_file = sumo_cfg_file
        self.gui = gui
        self.process_id = process_id
        # 仿真路网对象
        self.sumo_net_instance = None
        # 存储卡口、雷达经纬度所在路网的路段和位置信息
        self.kakou_pos_dict = {}
        self.init_sumo()
        self.simu_json_list = []
        self.simu_json_list_before = []

        self.sn_car_info_dict = {}
        self.RADAR_sn_set = set(RADAR_DATA_LIST.keys())
        # 存储每一辆车的多帧信息
        self.per_car_info_dict = {}
        # 下游感知车辆已经与上游仿真车辆匹配成功的车辆列表
        self.match_info_dict = {}
        for sn in self.RADAR_sn_set:
            self.sn_car_info_dict[sn] = {}
            self.per_car_info_dict[sn] = {}
            self.match_info_dict[sn] = {}

        self.add_car_station_info = {}

        # 是否开启速度控制
        self.speed_control = False
        # 正在加速列表
        self.speed_accelerate_list = []
        # 正在减速列表
        self.speed_decelerate_list = []

        self.executor = concurrent.futures.ThreadPoolExecutor(max_workers=300)
        self.global_vehicles = {}
        self.batch_num = 0
        self.global_time = time.time()
        self.global_num = 0
        self.son_process = {}
        self.add_count = 0
        # 缓存车辆创建命令执行道车辆创建成功的时间差
        self.add_time_dict = {}

    def init_sumo(self):
        """
        初始化sumo  加载
        :return:
        """
        # 加载路网
        self.sumo_net_instance = sumolib.net.readNet(self.sumo_net_file, withInternal=True, withLatestPrograms=True)
        # 启动仿真
        sumo_utils.start_sumo(self.sumo_cfg_file, False, gui=self.gui)
        # 设置所有车道的最大通行速度
        for id in traci.lane.getIDList():
            traci.lane.setMaxSpeed(id, MAX_SPEED)
        # 新增区域中心矩形，用于订阅100KM范围内的所有仿真车辆信息，并配置监听信息内容
        traci.polygon.add('GLOBAL_POLYGON_CENTER', GLOBAL_POLYGON_CENTER, (1, 0, 1))
        traci.polygon.subscribeContext('GLOBAL_POLYGON_CENTER',
                                       tc.CMD_GET_VEHICLE_VARIABLE, 1000000,
                                       [tc.VAR_SPEED, tc.VAR_TYPE, tc.VAR_POSITION, tc.VAR_ANGLE, ])

        # 将雷达、卡口相机的经纬度和终点位置转为路网上所在的路段id 位置距离 车道号，起始点和终点所在的路段id添加路由
        special_gantries = {
            12: {"arrive_edge_id": "-56"},
            16: {"arrive_edge_id": "-65"},
            22: {"arrive_edge_id": "-0"}
        }
        for sn, sn_info in RADAR_DATA_LIST.items():
            # 转换经纬度到路网坐标
            center_x, center_y = traci.simulation.convertGeo(sn_info['lon'], sn_info['lat'], fromGeo=True)
            EdgeID, Pos, LaneIndex = traci.simulation.convertRoad(sn_info['lon'], sn_info['lat'], True)

            if sn_info['type'] == 'hm':
                next_edge_id = EdgeID
                next_pos = Pos
            else:
                d_pos = LIDAR_RANGE
                next_edge_id, next_pos = sumo_utils.get_downstream_position_simplified(self.sumo_net_instance, EdgeID,
                                                                                       Pos, d_pos)
            # logger.info(f"{sn} 发车位置{d_pos}")

            if sn_info['sn'] <= 18:
                arrive_edge_id = "-38"
            else:
                arrive_edge_id = "-1"
            if 'arrive' in sn_info.keys():
                arrive_edge_id = sn_info['arrive']

            # next_edge_id, next_pos = sumo_utils.get_downstream_position_simplified(self.sumo_net_instance, EdgeID, Pos,
            #                                                                        LIDAR_RANGE)
            # if sn_info['sn'] <= 18:
            #     arrive_edge_id = "-38"
            # else:
            #     arrive_edge_id = "-1"
            routeId2 = None
            next_pos_2 = None
            next_edge_id_2 = None
            if sn in special_gantries:
                # 转换经纬度到路网坐标
                center_x, center_y = traci.simulation.convertGeo(sn_info['lon'], sn_info['lat'], fromGeo=True)
                EdgeID, Pos, LaneIndex = traci.simulation.convertRoad(sn_info['lon'], sn_info['lat'], True)
                # 计算下行位置（与第一套路由保持一致）
                next_edge_id_2, next_pos_2 = sumo_utils.get_downstream_position_simplified(
                    self.sumo_net_instance, EdgeID, Pos, LIDAR_RANGE)
                # 获取该门架对应的目标路段ID
                special_info = special_gantries[sn]
                arrive_edge_id_2 = special_info["arrive_edge_id"]
                # 创建第二套路由的标识符
                route_id = f"{sn_info['sn']}_2"
                # 添加特殊路由
                traci.route.add(route_id, [str(next_edge_id_2), arrive_edge_id_2])
                routeId2 = route_id
            logger.info(
                f"门架{sn_info['sn']} 所在路段为{EdgeID} 位置为{Pos} 下行100米后的路段为{next_edge_id} 位置为{next_pos}")
            if sn_info['sn'] not in self.kakou_pos_dict.keys():
                traci.route.add(str(sn_info['sn']), [str(next_edge_id), arrive_edge_id])

                self.kakou_pos_dict[str(sn_info['sn'])] = {"EdgeID": next_edge_id, "EdgeID2": next_edge_id_2,
                                                           "Pos": next_pos, "Pos2": next_pos_2,
                                                           'LaneIndex': LaneIndex,
                                                           'routeId': str(str(sn_info['sn'])),
                                                           'routeId2': routeId2}

            # 创建圆形区域，表示相机或雷达的位置，用于在使用gui可视化时，展示设备具体位置
            if sn_info['normal']:
                # 创建圆形区域
                self.create_circle_polygon(sn, center_x, center_y, radius=25)
            else:
                self.create_circle_polygon(sn, center_x, center_y, radius=25, color=(255, 0, 0, 255))

    def create_circle_polygon(self, vehID, center_x, center_y, radius, color=(0, 255, 0, 255), filled=True):
        """
        在指定位置创建一个近似圆形的多边形。

        :param vehID: 车辆 ID 或其他唯一标识符，用于命名多边形
        :param center_x: 圆心的 x 坐标
        :param center_y: 圆心的 y 坐标
        :param radius: 圆的半径
        :param color: 多边形的颜色 (R,G,B,A)，默认绿色不透明
        :param filled: 是否填充多边形，默认 True
        """
        # 生成圆周上的点
        num_points = 50  # 点的数量越多，圆越光滑
        points = []
        for i in range(num_points):
            angle = 2 * math.pi * i / num_points
            x = center_x + radius * math.cos(angle)
            y = center_y + radius * math.sin(angle)
            points.append((x, y))

        # 添加多边形
        polygon_id = f"circle_{vehID}"
        traci.polygon.add(polygon_id, shape=points, color=color, fill=filled, layer=0)

    def control_car(self):
        # self.add_count += re_queue_data.qsize()
        # logger.info(f"加车队列数量{re_queue_data.qsize()}")
        # vehiclePicInfo = None

        while not re_queue_data.empty():
            data = re_queue_data.get()
            if not data:
                continue
            sn_set = set()
            one_list = []
            if 'targetList' not in data.keys():
                continue
            e1FrameParticipant_list = data['targetList']
            ts = int(time.time() * 1000)
            for one in e1FrameParticipant_list:
                sourceId = one['station']
                one['sourceId'] = sourceId
                # if sourceId % 2 != 0:
                sn_set.add(sourceId)
                # if sourceId != 34:
                car_id = one['id']
                if car_id not in self.per_car_info_dict[sourceId].keys():
                    self.per_car_info_dict[sourceId][car_id] = {}
                    self.per_car_info_dict[sourceId][car_id][ts] = one
                else:
                    self.per_car_info_dict[sourceId][car_id][ts] = one

            continuous_vehicles, disappeared_vehicles, delete_vehicles = find_vehicles_by_duration_optimized(
                self.per_car_info_dict, 2000, 1200, 50000)
            for sn, disappeared_vehicle_list in disappeared_vehicles.items():
                for disappeared_vehicle in disappeared_vehicle_list:
                    dis_id = disappeared_vehicle[0]
                    # print(list(per_car_info_dict[sn][dis_id].keys())[-1] - int(time.time()* 1000))
                    if dis_id not in self.sn_car_info_dict[sn].keys():
                        self.sn_car_info_dict[sn][dis_id] = disappeared_vehicle[3]
                        car_one = disappeared_vehicle[3]
                        car_one['id'] = str(car_one['id'])
                        # logger.info(f"新增车辆{car_one}")
                        disappeared_vehicle[3]['error_sn'] = []
                        # count_add += 1

                        self.add_camera_car(disappeared_vehicle[3], sn)

            for c_id, s_id in delete_vehicles.items():
                # logger.info(f"删除车辆{c_id} {per_car_info_dict[s_id].keys()}  {sn_car_info_dict[s_id].keys()}")
                if c_id in self.per_car_info_dict[s_id].keys():
                    del self.per_car_info_dict[s_id][c_id]
                    # logger.info(f"per_car_info_dict 删除成功{c_id}")
                if c_id in self.sn_car_info_dict[s_id].keys():
                    del self.sn_car_info_dict[s_id][c_id]
                    # logger.info(f"sn_car_info_dict 删除成功{c_id}")
            error_sn_set = self.RADAR_sn_set - sn_set
            # logger.error(f"故障雷达{error_sn_set}")
            for sn in self.RADAR_sn_set:
                if sn in error_sn_set:
                    RADAR_DATA_LIST[sn]['normal'] = False
                else:
                    RADAR_DATA_LIST[sn]['normal'] = True

    def add_camera_car(self, vehiclePicInfo, sn):

        # try:
        car_id = vehiclePicInfo['id']
        # print(car_id)

        c_sourceId = vehiclePicInfo['station']
        # if c_sourceId in self.match_info_dict.keys() and car_id in self.match_info_dict[c_sourceId].keys():
        #     logger.info(f"仿真id可以修改")
        if c_sourceId in self.match_info_dict.keys() and car_id in self.match_info_dict[c_sourceId].keys():
            car_id = str(self.match_info_dict[c_sourceId][car_id]['id'])

            # logger.info(f"仿真id可以修改 {car_id} 改为 {str(self.match_info_dict[c_sourceId][car_id]['id'])}")
        # print("新增车辆",str(car_id),self.kakou_pos_dict[str(sn)]['routeId'],self.kakou_pos_dict[str(sn)]['Pos'],)
        set_speed = int(vehiclePicInfo['speed'] / 100 if (40 / 3.6) < vehiclePicInfo[
            'speed'] / 100 < (140 / 3.6) else 120 / 3.6)
        set_speed = vehiclePicInfo['speed'] / 3.6 if 30 <= vehiclePicInfo['speed'] <= 140 else 80 / 3.6
        # print(self.kakou_pos_dict[str(sn)]['Pos'] + random.randint(0,20))
        _, pos_hm, LaneIndex = traci.simulation.convertRoad(vehiclePicInfo['lon'],
                                                       vehiclePicInfo['lat'],
                                                       True)
        route_type = "main"
        if str(sn) not in ["12", "22", "16"]:
            routeId = self.kakou_pos_dict[str(sn)]['routeId']
            pos = self.kakou_pos_dict[str(sn)]['Pos']
        else:
            if LaneIndex != 0:
                routeId = self.kakou_pos_dict[str(sn)]['routeId']
                pos = self.kakou_pos_dict[str(sn)]['Pos']

            else:
                routeId = self.kakou_pos_dict[str(sn)]['routeId2']
                pos = pos_hm
                route_type = "zadao"
        self.add_time_dict[str(car_id)] = {"start":int(time.time() * 1000)}
        try:
            traci.vehicle.add(
                vehID=str(car_id),
                routeID=routeId,
                departPos=pos,
                typeID=1, depart='0',
                departSpeed=str(set_speed),
                departLane=str(LaneIndex))
        except Exception as e:
            logger.info(f"加车错误{e}  {LaneIndex}")
            if 'Invalid departlane' in str(e):
                traci.vehicle.add(
                    vehID=str(car_id),
                    routeID=routeId,
                    departPos=pos,
                    typeID=1, depart='0',
                    departSpeed=str(set_speed),
                    departLane='random')
        traci.vehicle.setSpeed(str(car_id),
                               set_speed)
        # traci.vehicle.slowDown(str(car_id),int(120 / 3.6),2)

        vehiclePicInfo['id'] = str(car_id)
        vehiclePicInfo['sourceId'] = sn
        vehiclePicInfo['route_type'] = route_type
        traci.vehicle.setParameter(str(car_id), "etc_info",
                                   str(vehiclePicInfo))
        # except Exception as e:
        #     logger.error(f"加车异常{str(e)}")

    def stop_remove_car(self, global_vehicles, queue_data):
        # print(queue_data.qsize())

        simu_json_list = []
        t_start = int(time.time() * 1000)
        for vehicle_id, veh_info in dict(global_vehicles).items():
            if vehicle_id in self.add_time_dict.keys() and "success" not in self.add_time_dict[vehicle_id].keys():
                logger.info(f"{vehicle_id} 创建耗时{int(time.time()  * 1000) - self.add_time_dict[vehicle_id]['start']}")
                self.add_time_dict[vehicle_id]['success'] = ""
            pos = veh_info[tc.VAR_POSITION]
            longitude, latitude = traci.simulation.convertGeo(pos[0], pos[1], fromGeo=False)
            speed = veh_info[tc.VAR_SPEED]
            try:
                etc_info = eval(traci.vehicle.getParameter(vehicle_id, "etc_info"))
            except Exception as e:
                # logger.info("etc_info", e)
                pass
            sourceId = etc_info['station']
            if sourceId in RADAR_DATA_LIST.keys():
                target_next_id = None
                # print(etc_info['route_type'])
                if etc_info['route_type'] == "main":
                    while RADAR_DATA_LIST[sourceId]['Downstream'] is not None:
                        # print(RADAR_DATA_LIST[sourceId]['Downstream']['sn'] in  etc_info['error_sn'].copy())
                        if RADAR_DATA_LIST[sourceId]['Downstream']['sn'] not in etc_info['error_sn']:
                            target_next_id = RADAR_DATA_LIST[sourceId]['Downstream']['sn']
                            break
                        sourceId = RADAR_DATA_LIST[sourceId]['Downstream']['sn']
                else:
                    while RADAR_DATA_LIST[sourceId]['HMDownstream'] is not None:
                        # print(RADAR_DATA_LIST[sourceId]['Downstream']['sn'] in  etc_info['error_sn'].copy())
                        if RADAR_DATA_LIST[sourceId]['HMDownstream']['sn'] not in etc_info['error_sn']:
                            target_next_id = RADAR_DATA_LIST[sourceId]['HMDownstream']['sn']
                            break
                    # sourceId = RADAR_DATA_LIST[sourceId]['Downstream']['sn']

                if target_next_id is not None:

                    # if int(vehicle_id.split("-")[0]) in self.per_car_info_dict[target_next_id].keys():
                    #     logger.info(f"{vehicle_id}存在下游感知车辆{self.per_car_info_dict[target_next_id].keys()}")
                    dis_next_sn = calculate_distance(longitude, latitude,
                                                     RADAR_DATA_LIST[target_next_id]['lon'],
                                                     RADAR_DATA_LIST[target_next_id]['lat'])

                    # if etc_info['route_type'] != "main":
                    #     print(vehicle_id,sourceId, target_next_id, dis_next_sn)
                    if etc_info['route_type'] == "main":
                        target_range = LIDAR_RANGE
                    else:
                        target_range = HM_LIDAR_RANGE
                    if target_range < dis_next_sn < target_range + PINJIE_DIS and "match" not in etc_info.keys():
                        # logger.info(f"{vehicle_id}允许匹配")
                        target_car_info, dis_other, dis_target = get_next_target_car_info_jingtai(t_start,
                                                                                                  target_next_id,
                                                                                                  longitude,
                                                                                                  latitude,
                                                                                                  self.per_car_info_dict,
                                                                                                  self.match_info_dict,
                                                                                                  speed, vehicle_id)
                        if target_car_info:
                            logger.info(
                                f"仿真车辆{vehicle_id} 速度{speed}匹配成功感知车辆{target_car_info['id']} 速度{target_car_info['speed'] / 3.6} 距离{dis_other} 2秒内仿真车需要行驶的距离为{dis_target}")
                            # target_car_info['error_sn'] = list(etc_info['error_sn'])
                            # target_car_info['row_sourceId'] = target_car_info['sourceId']
                            # target_car_info['sourceId'] = etc_info['sourceId']
                            etc_info['match'] = 1
                            _, _, LaneIndex = traci.simulation.convertRoad(target_car_info['lon'],
                                                                           target_car_info['lat'],
                                                                           True)
                            traci.vehicle.changeLaneRelative(str(vehicle_id), LaneIndex, 2.5)  # 3秒内完成换道
                            traci.vehicle.setSpeed(vehicle_id, dis_target / 2.5)
                            # if dis_other < 5:
                            #     target_car_info['close'] = 1
                            traci.vehicle.setParameter(str(vehicle_id), "etc_info",
                                                       str(etc_info))
                            # etc_info[]
                            # simu_is_front = is_in_front_jingtai(etc_info, target_car_info)
                            # logger.info(f"{vehicle_id} 在下游匹配车辆前方{simu_is_front} 下游sn{target_next_id}")
                            self.match_info_dict[target_next_id][str(target_car_info['id'])] = etc_info

                            match_queue_data.put(self.match_info_dict.copy())
                    if dis_next_sn <= target_range:
                        try:
                            traci.vehicle.remove(vehicle_id)
                        except Exception as e:
                            logger.info("stop", e)

            else:
                logger.error(f"雷达 {sourceId}不在配置文件内")

            # traci.vehicle.remove(vehicle_id) if int(speed * 3.6) < 20 else 1
            vehicle_type = veh_info[tc.VAR_TYPE]
            courseAngle = veh_info[tc.VAR_ANGLE]

            laneNum = 0
            # temp = {"orgCode": "simu", "id": int(etc_info['id'].split("-")[0]), "carType": etc_info['carType'],
            #         "carColor": etc_info['carColor'], "lon": longitude, "lat": latitude,
            #         "altitude": etc_info['altitude'], "speed": speed * 3.6, "angle": round(courseAngle, 1),
            #         "station": etc_info['station'], "lane": etc_info['lane'],
            #         "enGap": 0, "stakeNum": etc_info['stakeNum'], "speedAvg": 0, "passTime": 0,
            #         "firstReceiveTime": time_str(int(time.time() * 1000)), "disBefore": 0
            #         }
            temp = {"orgCode": etc_info['orgCode'], "id": int(vehicle_id.split("-")[0]), "carType": etc_info['carType'],
                    "lon": longitude, "lat": latitude, "speed": speed * 3.6, "angle": round(courseAngle, 1),
                    "station": etc_info['station'], "lane": etc_info['lane'],
                    "enGap": 0, "speedAvg": 0, "passTime": 0,
                    "firstReceiveTime": time_str(int(time.time() * 1000)), "disBefore": 0
                    }
            # flag = 1
            # if "match" in etc_info.keys():
            #     flag = 999
            # print("id",etc_info['id'].split("-")[0])
            # temp = {"id": etc_info['id'], "type": 1, 'source': 4,
            #         "laneNum": 0,
            #         "longitude": longitude, "latitude": latitude, "speed": int(speed * 3.6),
            #         "courseAngle": round(courseAngle, 1), 'plate': "默A00000", "flag": flag
            #         }
            simu_json_list.append(temp)

        # frame_info = {"orgCode": "simu", "frameNum": self.batch_num, "globalTime": time_str(int(time.time() * 1000)),
        #               "targetList": simu_json_list, }
        # print("仿真端", len(simu_json_list))
        if len(simu_json_list) > 0:
            queue_data.put(simu_json_list)

    def main_loop(self, stop_data, time_data):
        global re_queue_data, kafka_queue, match_queue_data, static_queue_data

        # 存储kafka心跳数据，超时重启kafka消费端
        kafka_queue = Queue(maxsize=2000)
        # 存储仿真实时轨迹
        queue_data = Queue(maxsize=2000)  # 多进程队列在此处创建
        # Process(target=send_kafka_1, args=(queue_data, self.process_id,)).start()
        # 在子进程中初始化多进程队列和资源
        re_queue_data = Queue(maxsize=2000)  # 多进程队列在此处创建
        # 存储仿真与下游感知队列匹配成功的数据
        match_queue_data = Queue(maxsize=2000)  # 多进程队列在此处创建
        # 存储统计信息
        static_queue_data = Queue(maxsize=2000)
        kafka_time_dict = {}

        kafka_time_dict[KAFKA_E1_LIST] = int(time.time())
        son_process = Process(target=read_trafficE1_channel,
                              args=(KAFKA_E1_LIST, re_queue_data, kafka_queue, queue_data, match_queue_data,
                                    static_queue_data),
                              daemon=True)
        son_process.start()
        self.son_process[KAFKA_E1_LIST] = son_process
        # son_process = Process(target=senf_kafka_E1,
        #                       args=(queue_data, self.process_id), daemon=True)
        # son_process.start()
        # self.son_process['send'] = son_process
        low_frame_num = 0
        batch_num = 0
        all_set = set()

        while True:
            # try:
            start_time = time.time()
            start_time_m = int(time.time() * 1000)

            # ================================收到门架上报数据，进行车辆处理==========================================

            global_vehicles = traci.polygon.getContextSubscriptionResults('GLOBAL_POLYGON_CENTER')
            self.control_car()
            # self.executor.submit(self.control_car)
            self.stop_remove_car(global_vehicles, queue_data)
            self.batch_num = batch_num

            batch_num += 1
            traci.simulationStep()
            end_time = time.time()
            all_set.update(set(global_vehicles.keys()))

            if end_time - self.global_time > 1:
                logger.info(f"仿真数量总数{len(all_set)}")
                statisticInfo = self.get_statisticInfo(global_vehicles.copy())
                static_queue_data.put(statisticInfo)
                while kafka_queue.qsize() > 0:
                    kafka_info = kafka_queue.get()
                    kafka_topic = kafka_info[0]
                    ts = kafka_info[1]
                    kafka_time_dict[kafka_topic] = ts

                if abs(kafka_time_dict[KAFKA_E1_LIST] - end_time) > 300:
                    if KAFKA_E1_LIST in self.son_process.keys():
                        logger.info(f"topic为{KAFKA_E1_LIST} 的接收进程,超过两分钟未收到数据，重启服务")
                        s_p = self.son_process[KAFKA_E1_LIST]
                        s_p.terminate()
                        s_p.join()
                        son_process = Process(target=read_trafficE1_channel,
                                              args=(
                                                  KAFKA_E1_LIST, re_queue_data, kafka_queue, queue_data,
                                                  match_queue_data),
                                              daemon=True)
                        son_process.start()
                        self.son_process[KAFKA_E1_LIST] = son_process

                time_data.put(int(time.time()))
                simu_frame = abs(self.global_num - batch_num)
                logger.info(f"进程{self.process_id} 仿真帧率 {simu_frame} 仿真数量{len(global_vehicles)}")
                if simu_frame < 7:
                    low_frame_num += 1
                if low_frame_num > 10 or (self.global_num > 8000 and len(global_vehicles) == 0):
                    logger.info(f"关闭子进程")
                    for s_name, s_p in self.son_process.items():
                        s_p.terminate()
                        s_p.join()
                    stop_data.put(1)
                self.global_time = end_time
                self.global_num = batch_num
            if start_time and (end_time - start_time) < 0.1:
                # print("耗时",(end_time - start_time))
                time.sleep(0.1 - (end_time - start_time))
            else:
                if start_time:
                    # logger.info(f"单帧耗时大于0.1s {end_time - start_time}")
                    pass
            # except Exception as e:
            #     logger.error(f"主循环异常{str(e)}")
            #     continue

    def get_statisticInfo(self, global_vehicles):
        statisticInfo = {}
        statisticInfo['totalNum'] = len(global_vehicles)
        type_count_dict = {}
        speed_list = []
        for vehicle_id, veh_info in dict(global_vehicles).items():
            speed = veh_info[tc.VAR_SPEED]
            speed_list.append(speed)
            try:
                etc_info = eval(traci.vehicle.getParameter(vehicle_id, "etc_info"))
            except Exception as e:
                continue
            carType = etc_info['carType']
            if carType not in type_count_dict.keys():
                type_count_dict[carType] = 1
            else:
                type_count_dict[carType] = type_count_dict[carType] + 1
        meanSpeed = np.mean(speed_list) * 3.6 if len(speed_list) > 0 else 0
        totalNumTypeInfo = []
        for t, c in type_count_dict.items():
            totalNumTypeInfo.append({'carType': t, 'carNum': c})
        statisticInfo['meanSpeed'] = round(meanSpeed, 1)
        statisticInfo['totalNumTypeInfo'] = totalNumTypeInfo
        return statisticInfo


def run_sumo_simulation(sumo_net_file, sumo_cfg_file, gui, process_id, stop_data, time_data):
    sumo_main = SumoMain(sumo_net_file, sumo_cfg_file, gui, process_id)
    sumo_main.main_loop(stop_data, time_data)


if __name__ == '__main__':
    # # 路网文件
    # sumo_net_file = "/sumo/net/jingtai.net.xml0611"
    # # 配置文件
    # sumo_cfg_file = "/sumo/net/jingtai.sumocfg"
    # 路网文件
    sumo_net_file = "./net/jingtai.net.xml"
    # 配置文件
    sumo_cfg_file = "./net/jingtai.sumocfg"
    # 是否启用gui可视化界面，调试阶段启用，线上部署关闭减少资源占用
    stop_data = Queue(maxsize=2000)  # 创建队列，仿真循环里面判断，如果仿真帧率小于7或者仿真路网上仿真车流数量为零，向队列中添加数据
    time_data = Queue(maxsize=2000)  # 创建队列，仿真主循环里一秒向里面添加一次系统时间戳
    gui = USE_GUI
    for i in range(1):
        logger.info("开启线程")
        tess_process = Process(target=run_sumo_simulation,
                               args=(sumo_net_file, sumo_cfg_file, gui, i, stop_data, time_data))
        tess_process.start()
        time_simu = int(time.time())
        while True:
            stop_flag = False
            # 如果仿真的最新时间与当前系统的时间的时间差大于一分钟，认为仿真异常关闭，需要重启服务
            while time_data.qsize() > 0:
                time_simu = time_data.get()
            if time_simu:
                # logger.info(f"时间差{abs(time_simu - int(time.time()))}")
                if abs(time_simu - int(time.time())) > 60:
                    stop_flag = True
                    time_simu = int(time.time())
            # 如果仿真帧率小于7或者仿真路网上仿真车流数量为零，认为仿真异常，需要重启服务
            while stop_data.qsize() > 0:
                _ = stop_data.get()
                stop_flag = True
            if stop_flag:
                logger.info("关闭开启线程")
                tess_process.terminate()
                tess_process.join()
                logger.info("重新开启线程")
                tess_process = Process(target=run_sumo_simulation,
                                       args=(sumo_net_file, sumo_cfg_file, gui, i, stop_data, time_data))
                tess_process.start()
            time.sleep(1)
